%% dynamics_coriolis_matrix.m
% @brief: compute coriolis matrix from dynamic equation
%         for each joint: coriolis matrix is 7x7, coriolis vector is 7x1
%         		[c11, ..., c17; ...; c71, ..., c77] * [qd1, qd2, qd3, qd4, qd5, qd6, qd7]' = C
%         thus, for manipulator with 7 joints, the coriolis matrix should be 7x7x7
function C = dynamics_gen_coriolis_func(TAU)
%% LINEARIZATION
C = sym(zeros(7, 7, 7));
for ii = 1:7	% for each element in inertia vector
	Q_ = zeros(7, 1);
	Q_(ii) = 1;
    for jj = 1:7	% for each joint
		C(jj, ii, ii) = subs(TAU(jj), ...
		                    {'dQ1', 'dQ2', 'dQ3', 'dQ4', 'dQ5', 'dQ6', 'dQ7', ...
				             'ddQ1', 'ddQ2', 'ddQ3', 'ddQ4', 'ddQ5', 'ddQ6', 'ddQ7', ...
							 'fe1', 'fe2', 'fe3', 'ne1', 'ne2', 'ne3', 'g', ...
							 'fv1', 'fv2', 'fv3', 'fv4', 'fv5', 'fv6', 'fv7', ...
				             'fc1', 'fc2', 'fc3', 'fc4', 'fc5', 'fc6', 'fc7'}, ...
							 {Q_(1), Q_(2), Q_(3), Q_(4), Q_(5), Q_(6), Q_(7), ...
							  0, 0, 0, 0, 0, 0, 0, ...
							  0, 0, 0, 0, 0, 0, 0, ...
							  0, 0, 0, 0, 0, 0, 0, ...
							  0, 0, 0, 0, 0, 0, 0});
    end
end

for ii = 1:6	% for element other than diagonal in row, 1~6
	for jj = (ii+1):7	% for element other than diagonal in column, 2~7
		Q_ = zeros(7, 1);
		Q_(ii) = 1;
		Q_(jj) = 1;
		for kk = 1:7
			T_ = subs(TAU(kk), ...
					 {'dQ1', 'dQ2', 'dQ3', 'dQ4', 'dQ5', 'dQ6', 'dQ7', ...
					  'ddQ1', 'ddQ2', 'ddQ3', 'ddQ4', 'ddQ5', 'ddQ6', 'ddQ7', ...
					  'fe1', 'fe2', 'fe3', 'ne1', 'ne2', 'ne3', 'g', ...
					  'fv1', 'fv2', 'fv3', 'fv4', 'fv5', 'fv6', 'fv7', ...
					  'fc1', 'fc2', 'fc3', 'fc4', 'fc5', 'fc6', 'fc7'}, ...
					  {Q_(1), Q_(2), Q_(3), Q_(4), Q_(5), Q_(6), Q_(7), ...
					   0, 0, 0, 0, 0, 0, 0, ...
					   0, 0, 0, 0, 0, 0, 0, ...
					   0, 0, 0, 0, 0, 0, 0, ...
					   0, 0, 0, 0, 0, 0, 0});
			% C(kk, ii, jj) = sym(0.5) * (T_ - C(kk, ii, ii) - C(kk, jj, jj));
            C(kk, ii, jj) = T_ - C(kk, ii, ii) - C(kk, jj, jj);
		end
	end
end